from numpy import pi
import random

class wrapper:
  def __init__ (self, v_min, v_max, omega_min, omega_max):
    """
    Interfaces the bot movement and collisions with the bt's resptective brains.
    """
    # Grab some globals from another file
    self.v_min = v_min
    self.v_max = v_max
    self.omega_min = omega_min
    self.omega_max = omega_max
  
  def state_conversion (self, bot, collisions, n_angles, angle_index):
    """
    7/17/08
    Translate sensor readings (nearby surfaces's angles from bot forward
    movement directions) into discrete angles and then to a numerical state
    representation (this format is suitable for the learning algorithm).
    As of 7/17/08 there is no distinguishing between walls and bots.
    Inputs: bot, collisions (what it's touching), n_angles, element in collisions w/angle info
    Output: State (number)
    """
    state = 0
    for collision in collisions:
      if collision[0] != bot:
        print 'Error: bot in collision list does not correspond.'
        continue
      # Find the angle wedge the collision was on
      wedge = int (collision[angle_index]*n_angles/(2*pi)) % n_angles
      # set that bit!
      state = state | (1 << wedge)
    return state

  def reset_speed_and_time (self, bot):
    """
    Resets the speed and clock of a bot. This is necessary when changing
    the bot movement speeds and type (e.g. rotate); otherwise the bot won't 
    move in a straight line, and the distance calculations (v*t <--t not reset)
    get distorted.
    Inputs: Bot
    Outputs: None
    """
    bot.v = 0.
    bot.omega = 0.
    bot.t = 0.

  def new_velocity (self, v_min, v_max):
    """
    Calculate a new velocity.
    Inputs: v_min, v_max
    Output: velocity
    """
    return v_min + (v_max-v_min)*random.random ()

  def new_rot_rate (self, bot, omega_min, omega_max):
    """
    Calculate a new angular velocity.
    Inputs: omega_min, omega_max
    Output: omega
    """
    #if random.random () < 0.5: 
    #  omega = omega_max
    #else:
    #  omega = -omega_max
    if (bot.phif - bot.phi)%(2*pi) < pi + 0.01:
      omega = omega_max
    else:
      omega = -omega_max
    return omega
  
  def action_conversion (self, bot, action, n_angles, n_actions):
    """
    Execute a given bot action based on the numerical action (i.e. convert
    policy output to bot action). Does not output information about the action
    to be executed, it *executes* the action.
    Inputs: bot, action (number), n_angles, n_actions
    Outputs: None
    """
  
    if action == 0:
      self.reset_speed_and_time (bot)
      bot.v = self.new_velocity (self.v_min, self.v_max)
      bot.move_state = 'forward'
    elif action < n_actions:
      self.reset_speed_and_time (bot)
      bot.phif = (bot.phi + action*2*pi/n_angles) % (2*pi)
      bot.omega = self.new_rot_rate (bot, self.omega_min, self.omega_max)
      bot.move_state = 'rotate'
    else:
      print 'Error: invalid action.'
      
class reward:
  def __init__ (self):
    self.move_factor = 0.1
    self.rotate_factor = -0.1
    self.c = -1.
  def move_reward (self, distance):
    # Discourage bumping into walls with the -1
    return self.c + distance*self.move_factor
    #return distance*self.move_factor
  def rotate_reward (self, theta):
    #return theta*self.rotate_factor
    return self.c

